/*
 * Date           Author       Notes
 * 2018-10-06     ZhaoXiaowei    the first version
 */

.macro SAVE_CONTEXT

	/* Switch to use the EL0 stack pointer. */
	MSR 	SPSEL, #0

	/* Save the entire context. */
	STP 	X0, X1, [SP, #-0x10]!
	STP 	X2, X3, [SP, #-0x10]!
	STP 	X4, X5, [SP, #-0x10]!
	STP 	X6, X7, [SP, #-0x10]!
	STP 	X8, X9, [SP, #-0x10]!
	STP 	X10, X11, [SP, #-0x10]!
	STP 	X12, X13, [SP, #-0x10]!
	STP 	X14, X15, [SP, #-0x10]!
	STP 	X16, X17, [SP, #-0x10]!
	STP 	X18, X19, [SP, #-0x10]!
	STP 	X20, X21, [SP, #-0x10]!
	STP 	X22, X23, [SP, #-0x10]!
	STP 	X24, X25, [SP, #-0x10]!
	STP 	X26, X27, [SP, #-0x10]!
	STP 	X28, X29, [SP, #-0x10]!
	STP 	X30, XZR, [SP, #-0x10]!

	MRS		X0, CurrentEL
	CMP		X0, 0xc
	B.EQ	3f
	CMP		X0, 0x8
	B.EQ	2f
	CMP		X0, 0x4
	B.EQ	1f
	B 		.
3:
	MRS		X3, SPSR_EL3
	/* Save the ELR. */
	MRS		X2, ELR_EL3
	B		0f
2:
	MRS		X3, SPSR_EL2
	/* Save the ELR. */
	MRS		X2, ELR_EL2
	B		0f
1:
	MRS		X3, SPSR_EL1
	MRS		X2, ELR_EL1
	B		0f
0:

	STP 	X2, X3, [SP, #-0x10]!

	MOV 	X0, SP   /* Move SP into X0 for saving. */

	/* Switch to use the ELx stack pointer. */
	MSR 	SPSEL, #1

	.endm

.macro SAVE_CONTEXT_T

	/* Switch to use the EL0 stack pointer. */
	MSR 	SPSEL, #0

	/* Save the entire context. */
	STP 	X0, X1, [SP, #-0x10]!
	STP 	X2, X3, [SP, #-0x10]!
	STP 	X4, X5, [SP, #-0x10]!
	STP 	X6, X7, [SP, #-0x10]!
	STP 	X8, X9, [SP, #-0x10]!
	STP 	X10, X11, [SP, #-0x10]!
	STP 	X12, X13, [SP, #-0x10]!
	STP 	X14, X15, [SP, #-0x10]!
	STP 	X16, X17, [SP, #-0x10]!
	STP 	X18, X19, [SP, #-0x10]!
	STP 	X20, X21, [SP, #-0x10]!
	STP 	X22, X23, [SP, #-0x10]!
	STP 	X24, X25, [SP, #-0x10]!
	STP 	X26, X27, [SP, #-0x10]!
	STP 	X28, X29, [SP, #-0x10]!
	STP 	X30, XZR, [SP, #-0x10]!

	MRS		X0, CurrentEL
	CMP		X0, 0xc
	B.EQ	3f
	CMP		X0, 0x8
	B.EQ	2f
	CMP		X0, 0x4
	B.EQ	1f
	B 		.
3:
	MRS		X3, SPSR_EL3
	MOV		X2, X30
	B		0f
2:
	MRS		X3, SPSR_EL2
	MOV		X2, X30
	B		0f
1:
	MRS		X3, SPSR_EL1
	MOV		X2, X30
	B		0f
0:

	STP 	X2, X3, [SP, #-0x10]!

	MOV 	X0, SP   /* Move SP into X0 for saving. */

	/* Switch to use the ELx stack pointer. */
	MSR 	SPSEL, #1

	.endm

.macro RESTORE_CONTEXT

	/* Switch to use the EL0 stack pointer. */
	MSR 	SPSEL, #0

	/* Set the SP to point to the stack of the task being restored. */
	MOV		SP, X0

	LDP 	X2, X3, [SP], #0x10  /* SPSR and ELR. */

	MRS		X0, CurrentEL
	CMP		X0, 0xc
	B.EQ	3f
	CMP		X0, 0x8
	B.EQ	2f
	CMP		X0, 0x4
	B.EQ	1f
	B 		.
3:
	MSR		SPSR_EL3, X3
	MSR		ELR_EL3, X2
	B		0f
2:
	MSR		SPSR_EL2, X3
	MSR		ELR_EL2, X2
	B		0f
1:
	MSR		SPSR_EL1, X3
	MSR		ELR_EL1, X2
	B		0f
0:

	LDP 	X30, XZR, [SP], #0x10
	LDP 	X28, X29, [SP], #0x10
	LDP 	X26, X27, [SP], #0x10
	LDP 	X24, X25, [SP], #0x10
	LDP 	X22, X23, [SP], #0x10
	LDP 	X20, X21, [SP], #0x10
	LDP 	X18, X19, [SP], #0x10
	LDP 	X16, X17, [SP], #0x10
	LDP 	X14, X15, [SP], #0x10
	LDP 	X12, X13, [SP], #0x10
	LDP 	X10, X11, [SP], #0x10
	LDP 	X8, X9, [SP], #0x10
	LDP 	X6, X7, [SP], #0x10
	LDP 	X4, X5, [SP], #0x10
	LDP 	X2, X3, [SP], #0x10
	LDP 	X0, X1, [SP], #0x10

	/* Switch to use the ELx stack pointer.  _RB_ Might not be required. */
	MSR 	SPSEL, #1

	ERET

	.endm

.text
/*
 * rt_base_t rt_hw_interrupt_disable();
 */
.globl rt_hw_interrupt_disable
rt_hw_interrupt_disable:
	MRS      X0, DAIF
	MSR      DAIFSet, #3
	DSB      SY
	RET

/*
 * void rt_hw_interrupt_enable(rt_base_t level);
 */
.globl rt_hw_interrupt_enable
rt_hw_interrupt_enable:
	DSB     SY
	MOV     X1, #0xC0
	ANDS    X0, X0, X1
	B.NE    rt_hw_interrupt_enable_exit
	MSR     DAIFClr, #3
rt_hw_interrupt_enable_exit:
	RET

/*
 * void rt_hw_context_switch_to(rt_uint32 to);
 * r0 --> to
 */
.globl rt_hw_context_switch_to
rt_hw_context_switch_to:
	LDR		X0, [X0]
	RESTORE_CONTEXT

.text
/*
 * void rt_hw_context_switch(rt_uint32 from, rt_uint32 to);
 * r0 --> from
 * r1 --> to
 */
.globl rt_hw_context_switch
rt_hw_context_switch:

	MOV		X8,X0
	MOV		X9,X1

	SAVE_CONTEXT_T
	
    STR		X0, [X8]            // store sp in preempted tasks TCB
    LDR		X0, [X9]            // get new task stack pointer
	
	RESTORE_CONTEXT

/*
 * void rt_hw_context_switch_interrupt(rt_uint32 from, rt_uint32 to);
 */
.globl rt_thread_switch_interrupt_flag
.globl rt_interrupt_from_thread
.globl rt_interrupt_to_thread
.globl rt_hw_context_switch_interrupt
rt_hw_context_switch_interrupt:
    ADR 	X2, rt_thread_switch_interrupt_flag
    LDR 	X3, [X2]
    CMP 	X3, #1
    B.EQ 	_reswitch
    ADR 	X4, rt_interrupt_from_thread   // set rt_interrupt_from_thread
    MOV 	X3, #1              // set rt_thread_switch_interrupt_flag to 1
    STR 	X0, [X4]
    STR 	X3, [X2]
_reswitch:
    ADR 	X2, rt_interrupt_to_thread     // set rt_interrupt_to_thread
    STR 	X1, [X2]
    RET

.text
    .align  8
.globl vector_fiq
vector_fiq:
    SAVE_CONTEXT
	STP 	X0, X0, [SP, #-0x10]!
    BL      rt_hw_trap_fiq
	LDP 	X0, X0, [SP], #0x10
    RESTORE_CONTEXT

.globl      rt_interrupt_enter
.globl      rt_interrupt_leave
.globl      rt_thread_switch_interrupt_flag
.globl      rt_interrupt_from_thread
.globl      rt_interrupt_to_thread

.globl      rt_current_thread

    .align  8
.globl vector_irq
vector_irq:
    SAVE_CONTEXT
	STP 	X0, X0, [SP, #-0x10]!

    BL      rt_interrupt_enter
    BL      rt_hw_trap_irq
    BL      rt_interrupt_leave
	
	LDP 	X0, X0, [SP], #0x10

    // if rt_thread_switch_interrupt_flag set, jump to
    // rt_hw_context_switch_interrupt_do and don't return
    ADR 	X1, rt_thread_switch_interrupt_flag
    LDR     X2, [X1]
    CMP     X2, #1
    B.NE     vector_irq_exit

    MOV     X2,  #0         // clear flag
    STR     X2,  [X1]

    ADR     X3,  rt_interrupt_from_thread
    LDR     X4,  [X3]
	STR     x0,  [X4]       // store sp in preempted tasks's TCB

    ADR     x3,  rt_interrupt_to_thread
    LDR     X4,  [X3]
    LDR     x0,  [X4]       // get new task's stack pointer
	
vector_irq_exit:	
	RESTORE_CONTEXT

    .align  8
    .globl  vector_error
vector_error:
    SAVE_CONTEXT
    BL      rt_hw_trap_error
    B       .
